import os
import launch
import launch_ros
import launch.event_handlers
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
    map_file = os.path.join(
        get_package_share_directory("nav_pkg"), "maps", "map.yaml"
    )

    nav_param_file = os.path.join(
        get_package_share_directory("nav_pkg"), "config", 
        "nav2_params_agv.yaml" # nav2_params.yaml
    )

    nav2_launch_dir = os.path.join(
        get_package_share_directory("nav2_bringup"), "launch"
    )

    navigation_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([nav2_launch_dir, "/bringup_launch.py"]),
        launch_arguments={
            "map": map_file,
            "use_sim_time": use_sim_time,
            "params_file": nav_param_file,
        }.items(),
    )

    rviz_file = os.path.join(
        get_package_share_directory("nav_pkg"), "rviz", "nav.rviz"
    )
    rviz_cmd = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_file],
    )
    # 机器人AMCL位姿节点
    robot_pose_subscriber_cmd = launch_ros.actions.Node(
        package="nav_pkg",
        executable="robot_pose_subscriber",
        name="robot_pose_subscriber",
    )
    # 自定义导航服务
    nav_server_cmd = launch_ros.actions.Node(
        package="nav_pkg", executable="nav_server", name="nav_server"
    )

    # 添加 map -> odom 的静态变换
    static_transform_publisher_cmd = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=["0", "0", "0", "0", "0", "0", "0", "map", "odom"],
        name="static_transform_publisher",
    )
    # 初始化机器人位姿
    initial_pose_publisher_cmd = Node(
        package="nav_pkg", executable="initialize_pose", name="initialize_pose"
    )

    ld = LaunchDescription()
    ld.add_action(navigation_cmd)
    ld.add_action(rviz_cmd)
    ld.add_action(robot_pose_subscriber_cmd)  # 机器人AMCL位姿节点
    ld.add_action(nav_server_cmd)
    ld.add_action(initial_pose_publisher_cmd)
    # ld.add_action(static_transform_publisher_cmd) # 如无map->odom的静态变换，请取消注释
    return ld
